#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import yaml
import os
from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseGoal
from std_msgs.msg import String
# Note: PoseWithCovarianceStamped is not used in this script, so it can be removed if not needed for future extensions.
# from geometry_msgs.msg import PoseWithCovarianceStamped 

def send_goal():
    """
    Initializes a ROS node to send navigation goals.
    It reads goals from a YAML file and sends them to the move_base action server
    based on received String messages on the /nav_goal_no topic.
    """
    rospy.init_node('send_goal', anonymous=True)
    
    # Get the path to the YAML file where goals are stored.
    # It assumes 'nav_goals.yaml' is in the same directory as this script.
    pkg_path = os.path.dirname(os.path.abspath(__file__))
    file_path = os.path.join(pkg_path, 'nav_goals.yaml')

    # Attempt to load goals from the YAML file.
    # If the file doesn't exist or is empty, appropriate error messages are logged.
    try:
        with open(file_path, 'r') as f:
            goals = yaml.safe_load(f)
            if goals is None: # Handle case where YAML file is empty
                rospy.logerr(f"The YAML file '{file_path}' is empty or not found. Please record goals first using the goal_recorder.")
                return
    except IOError: # Catch file not found or permission errors
        rospy.logerr(f"Could not open the YAML file '{file_path}'. Please ensure it exists and has goals recorded.")
        return

    # Create a publisher for the /move_base/goal topic.
    # This topic expects messages of type MoveBaseActionGoal.
    goal_pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=1)
    
    def goal_callback(msg):
        """
        Callback function for the /nav_goal_no topic.
        When a message is received, it retrieves the corresponding goal from the loaded YAML data
        and publishes it to move_base.
        """
        goal_name = msg.data # The message data is expected to be the name/ID of the goal
        if goal_name in goals:
            rospy.loginfo(f"Received goal request for: {goal_name}. Sending goal to navigation stack.")
            
            goal_data = goals[goal_name] # Retrieve the goal's pose data
            
            # Create a MoveBaseActionGoal message
            move_base_goal = MoveBaseActionGoal()
            move_base_goal.header.stamp = rospy.Time.now()
            move_base_goal.header.frame_id = "map" # Goals are typically defined in the 'map' frame
            
            # Fill in the target_pose part of the goal message
            pose_goal = move_base_goal.goal.target_pose
            pose_goal.header.stamp = rospy.Time.now()
            pose_goal.header.frame_id = "map"
            
            # Populate position details
            pose_goal.pose.position.x = goal_data['position']['x']
            pose_goal.pose.position.y = goal_data['position']['y']
            pose_goal.pose.position.z = goal_data['position']['z']
            
            # Populate orientation details (quaternion)
            pose_goal.pose.orientation.x = goal_data['orientation']['x']
            pose_goal.pose.orientation.y = goal_data['orientation']['y']
            pose_goal.pose.orientation.z = goal_data['orientation']['z']
            pose_goal.pose.orientation.w = goal_data['orientation']['w']
            
            # Publish the complete move_base goal
            goal_pub.publish(move_base_goal)
        else:
            # Log a warning if the requested goal name is not found in the YAML file
            rospy.logwarn(f"Goal '{goal_name}' not found in the YAML file.")
            
    # Subscribe to the /nav_goal_no topic, which triggers the goal_callback
    # This topic is expected to receive String messages containing the goal ID/name.
    rospy.Subscriber('/nav_goal_no', String, goal_callback)

    # Keep the node running until it's shut down (e.g., by Ctrl+C)
    rospy.spin()

if __name__ == '__main__':
    try:
        send_goal()
    except rospy.ROSInterruptException:
        # Catch exceptions that occur when the ROS system is interrupted (e.g., Ctrl+C)
        pass
